# -*- coding: utf-8 -*-
"""
Created on Sun Apr  3 20:14:31 2022

@author: lizhuang
"""
import numpy as np
from scipy import integrate
import sviewgui.sview as sv
import pandas as pd
from numpy import sin,cos
import matplotlib.pyplot as plt
from scipy.optimize import root,fsolve
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus']=False #用来正常显示负号
from 转动惯量和力矩试算 import 转动惯量和等效力矩
class 曲柄连杆():
    def __init__(self,连杆长度=1.2,曲柄长度=0.4,连杆质心=0.6,曲柄质心=0.2,曲柄质量=1.2,连杆质量=3.6,滑块质量=6, \
                 曲柄转动惯量=0.016,连杆转动惯量=0.45,滑块外力x=1000,滑块外力y=0):
        self.时间精度=1000#dt
        self.曲柄角速度=10*np.pi/180
        self.模拟时间长度=2*np.pi/self.曲柄角速度
        self.连杆长度=连杆长度
        self.曲柄长度=曲柄长度
        self.连杆质心=连杆质心
        self.曲柄质心=曲柄质心#质量心到旋转副距离
        self.曲柄质量=曲柄质量
        self.连杆质量=连杆质量
        self.滑块质量=滑块质量
        self.曲柄转动惯量=曲柄转动惯量
        self.连杆转动惯量=连杆转动惯量
        self.曲柄受的扭矩=0
        self.曲柄外力x=0
        self.曲柄外力y=0
        self.曲柄起点加速度x=0
        self.曲柄起点加速度y=0
        self.连杆外力x=0
        self.连杆外力y=0
        self.连杆受扭矩=0
        self.滑块外力x=滑块外力x
        self.滑块外力y=滑块外力y
        self.滑块和固定点延长线夹角=np.pi*3/2
        return
    def r(self,x):
        #Function to compute the acceleration of crank
        #Copyright 2002 Ou Xiuquan
        #Input parameters
        #
        #x1)=rj
        #x[2]=thetaj
        #x[3]=dthetaj
        #x[4]=ddthetaj
        #
        #Output parameters
        #
        #y[1]=Re[ddB]
        #y[2]=Im[ddB]
        #
        pi=np.pi;cos=np.cos;sin=np.sin
        ddB=np.array([[x[0]*x[3]*cos(x[1]+pi/2)+x[0]*(x[2]**2)*cos(x[1]+pi)],[x[0]*x[3]*sin(x[1]+pi/2)+x[0]*(x[2]**2)*sin(x[1]+pi)]]);
        y=ddB;
        return y
    def rdy(self,x):
        #Dynamic analysis for crank
        #Copyright 2003 Qu Xiuquan
        #Input parameters
        #
        #x[0]theta -i
        #x[1]=dtheta-i
        #x[2]=ddtheta-i
        #x[3]=RxB
        #x[4]=RyB
        #
        #Output parameters
        #
        #y[0]=RxA
        #y[1]=RyA
        #y[2]=M1
        #
        pi=np.pi;cos=np.cos;sin=np.sin
        g=9.8;
        ri=self.曲柄长度;rci=self.曲柄质心;
        mi=self.曲柄质量;Ji=self.曲柄转动惯量;
        Fxi=self.曲柄外力x;Fyi =self.曲柄外力y;Mi =self.曲柄受的扭矩;
        ReddA =self.曲柄起点加速度x;ImddA =self.曲柄起点加速度y;
        y=np.array([0.001,0.0001,0.0001])
        y[0]=mi*ReddA+mi*rci*x[2]*cos(x[0]+pi/2)+mi*rci*x[1]**2*cos(x[0]+pi)-Fxi+x[3];
        y[1]=mi*ImddA+mi*rci*x[2]*sin(x[0]+pi/2)+mi*rci*x[1]**2*sin(x[0]+pi)-Fyi+x[4]+mi*g;
        y[2]=Ji*x[2]-y[0]*rci*sin(x[0])+y[1]*rci*cos(x[0])-x[3]*(ri-rci)*sin(x[0])+x[4]*(ri-rci)*cos(x[0])-Mi;
        '''y[0]=mi*ReddA+mi*rci*x[2]*cos(x[0]+pi/2)+mi*rci*x[1]**2*cos(x[0]+pi)-Fxi-x[3];
        y[1]=mi*ImddA+mi*rci*x[2]*sin(x[0]+pi/2)+mi*rci*x[1]**2*sin(x[0]+pi)-Fyi-x[4]+mi*g;
        y[2]=Ji*x[2]-y[0]*rci*sin(x[0])+y[1]*rci*cos(x[0])+x[3]*(ri-rci)*sin(x[0])-x[4]*(ri-rci)*cos(x[0])-Mi;'''
        return y
    def RRP(self,x):
        #function to compute the acceleration for RRP bar group
        #
        #Copyright 2002 Qu Xiuquan
        #
        #Input parameters
        #
        #x[0]=i
        #x[1]=theta -i
        #x[2]=theta-j
        #x[3]=dtheta-i
        #x[4]=Re[ddB]
        #x[5]=Im[ddB]
        #x[6]=Re[ddK]
        #x[7]=Im[ddk]
        #x[8]=ds
        #Output parameters
        #
        #y[0]=ddthe地-i
        #y[1]=d地
        #
        pi=np.pi;cos=np.cos;sin=np.sin
        a=np.array([[x[0]*cos(x[1]+pi/2),-cos(x[2])],[x[0]*sin(x[1]+pi/2),-sin(x[2])]]);
        b=(-np.array([[x[0]*cos(x[1]+pi),0], \
                     [x[0]*sin(x[1]+pi),0]])).dot( \
            np.array([[x[3]**2],[x[8]]])) \
                +np.array([[x[6]-x[4]],[x[7]-x[5]]])
        y=np.linalg.inv(a).dot(b);
        return y
    def RRPdy(self,x):
        #Dynamic analysis for RRP bar group
        #Copyright 2003 Qu Xiuquan
        #Input parameters
        #x(1)=theta-i
        #x(2)=dtheta-i
        #x(3)=ddtheta-i
        #x(4)=dd-j
        #x(5)=Re[ddB]
        #x(6)=Im[ddB]
        #Output parameters
        #
        #y(1)=RxB
        #y(2)=RyB
        #y(3)=RxC
        #y(4)=RyC
        #y(5)=RD
        #
        pi=np.pi;cos=np.cos;sin=np.sin
        g=9.8;
        ri=self.连杆长度;rci=self.连杆质心;mi=self.连杆质量;mj=self.滑块质量;Ji= self.连杆转动惯量;
        Fxi=self.连杆外力x;Fyi=self.连杆外力y;Fxj=self.滑块外力x;Fyj=self.滑块外力y;Mi=self.连杆受扭矩;
        thj=self.滑块和固定点延长线夹角;#
        a =np.zeros((5,5));
        a[0,0]=1;a[0,2]=1;
        a[1,1]=1;a[1,3]=1;
        a[2,0]=rci*sin(x[0]);
        a[2,1]=-rci*cos(x[0]);
        a[2,2]=-(ri-rci)*sin(x[0]);
        a[2,3]=(ri-rci)*cos(x[0]);
        a[3,2]=-1;a[3,4]=-sin(thj);
        a[4,3]=-1;a[4,4]=cos(thj)
        b=np.zeros((5,1));
        b[0,0]=mi*x[4]+mi*rci*x[2]*cos(x[0]+pi/2)+mi*rci*x[1]**2*cos(x[0]+pi)-Fxi;
        b[1,0]=mi*x[5]+mi*rci*x[2]*sin(x[0]+pi/2)+mi*rci*x[1]**2*sin(x[0]+pi)-Fyi+mi*g;
        b[2,0]=Ji*x[2]-Mi;
        b[3,0]=mj*x[3]*cos(thj)-Fxj;
        b[4,0]=mj*x[3]*sin(thj)-Fyj+mj*g;
        y=np.linalg.inv(a).dot(b);
        return y
    def 位移方程(self,x):#x0=角度1，x2位移
        return np.array([self.曲柄长度*np.sin(self.thea)+self.连杆长度*np.sin(x[0]), \
                         self.曲柄长度*np.cos(self.thea)+self.连杆长度*np.cos(x[0])-x[1]])
    def 解方程(self):
        sol3_fsolve = fsolve(self.位移方程,self.初始)
        return sol3_fsolve
    def 转动惯量计算(self,曲柄角,连杆角):
        转动惯量=self.曲柄转动惯量+self.曲柄质量*self.曲柄质心**2+(self.连杆质量+self.滑块质量)*self.曲柄长度**2+ \
            (self.连杆质心)
        return 转动惯量
    def 曲柄连杆转动惯量表(self):
        数据=pd.DataFrame()
        角度=np.linspace(np.pi,-np.pi,10001)
        位移=np.linspace(0,2*np.pi,10001)
        时间=np.linspace(0,2*np.pi/self.角速度,10001)#时间
        for i in range(len(角度)):
            self.thea=角度[i]
            shuju=self.解方程()
            self.初始=shuju
            位移[i]=shuju[1]
        #最大位移=数据['位移'].max()
        转动惯量表=5
        return 转动惯量表
    def 转动惯量查询(self):
        return
    def f(self,x,a,b):#速度时间关系函数
        return a*x + b
    def 积分(self,积分距离,a,b):
        v = integrate.quad(self.f,0,积分距离,args=(a,b))
        return v[0]
    def 合成仿真(self):
        时间=np.linspace(0,self.模拟时间长度,self.时间精度)
        时间间隔=self.模拟时间长度/self.时间精度
        print(len(时间))
        数据=pd.DataFrame()
        数据['时间']=时间
        #rx=[rj,]
        曲柄角速度=0
        曲柄转角=self.滑块和固定点延长线夹角;theta2=self.滑块和固定点延长线夹角;dtheta2=-self.曲柄角速度/(self.连杆长度/self.曲柄长度);滑块速度=0;
        滑块位移=0
        for i in range(len(时间)):
            曲柄角加速度=0.001
            ddb=self.r(np.array([self.曲柄长度,曲柄转角,曲柄角速度,0]))
            数据.loc[i,'bx方向加速度']=float(ddb[0])
            数据.loc[i,'by方向加速度']=float(ddb[1])                       
            print(float(ddb[0]),float(ddb[1]))
            s=self.RRP(np.array([self.连杆长度,theta2,self.滑块和固定点延长线夹角,dtheta2,float(ddb[0]),float(ddb[1]),0,0,滑块速度]))
            数据.loc[i,'ddtheta2']=float(s[0])
            数据.loc[i,'dds']=float(s[1])
            print(i)
            print(s[0])
            rrpdy=self.RRPdy(np.array([theta2,dtheta2,float(s[0]),float(s[1]),float(ddb[0]),float(ddb[1])]))
            rdy=self.rdy(np.array([曲柄转角,曲柄角速度,0,-float(rrpdy[0]),-float(rrpdy[1])]))
            曲柄角速度=self.积分(时间间隔,0,曲柄角加速度)+曲柄角速度
            曲柄转角=self.积分(时间间隔,0,self.曲柄角速度)+曲柄转角
            dtheta2=self.积分(时间间隔,0,float(s[0]))+dtheta2     
            theta2=self.积分(时间间隔,0,dtheta2)+theta2 
            滑块速度=self.积分(时间间隔,0,float(s[1]))+滑块速度
            滑块位移=self.积分(时间间隔,0,float(滑块速度))+滑块位移
            数据.loc[i,'曲柄转角']=float(曲柄转角)*180/np.pi-270
            数据.loc[i,'滑块速度']=float(滑块速度)
            数据.loc[i,'滑块位移']=float(滑块位移)  
            数据.loc[i,'连杆角速度']=float(dtheta2)
            数据.loc[i,'连杆角']=float(theta2)
            数据.loc[i,'曲柄约束力bx'] = float(rdy[0])
            数据.loc[i,'曲柄约束力by'] = float(rdy[1])
            曲柄力=np.sqrt(rdy[0]**2+rdy[1]**2)
            数据.loc[i,'曲柄力'] = float(曲柄力)
            数据.loc[i,'曲柄扭矩']=float(rdy[2])
            数据.loc[i,'连杆支撑力bx']=float(rrpdy[0])
            数据.loc[i,'连杆支撑力by'] = float(rrpdy[1])
            数据.loc[i,'连杆支撑力cx'] = float(rrpdy[2])
            数据.loc[i,'连杆支撑力cy'] = float(rrpdy[3])
            数据.loc[i,'滑块侧向力'] = float(rrpdy[4])
        #数据.loc[i,'曲柄转角']=数据.loc[i,'曲柄转角']*180/np.pi-270
        return 数据
if __name__=="__main__":
    '''a=曲柄连杆(连杆长度=1.231,曲柄长度=0.175,连杆质心=0.6,曲柄质心=0.09,曲柄质量=10699,连杆质量=7350,滑块质量=25200, \
                 曲柄转动惯量=592,连杆转动惯量=1787,滑块外力x=0,滑块外力y=25000000)'''
    a=曲柄连杆(连杆长度=1.231,曲柄长度=0.175,连杆质心=0.6,曲柄质心=0.09,曲柄质量=0.034,连杆质量=1.94,滑块质量=62, \
                 曲柄转动惯量=9.3e-5,连杆转动惯量=0.25,滑块外力x=0,滑块外力y=50000)
    '''a=曲柄连杆(连杆长度=1.2,曲柄长度=0.4,连杆质心=0.6,曲柄质心=0.2,曲柄质量=1.2,连杆质量=3.6,滑块质量=6, \
                 曲柄转动惯量=0.016,连杆转动惯量=0.45,滑块外力x=1000,滑块外力y=0)'''
    y=a.合成仿真()
    #sv.buildGUI(y)
    #plt.plot(y.曲柄转角,y.rdy)
    #plt.show()
    y.滑块位移=y.滑块位移-min(y.滑块位移)
    DATA =y ## Put Your DataFrame Object name here ##  

    #- Axes Setting ---------- 
    fig, ax = plt.subplots()
    ax.set_xlabel( "曲柄转角")
    ax.set_ylabel( "滑块位移" )
    ax.set_xlim(min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    ax.set_ylim( min(DATA['滑块位移'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['滑块位移'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['滑块位移'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['滑块位移'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    #- PLOT ------------------ 
    ax.scatter(DATA["曲柄转角"].replace([np.inf, -np.inf], np.nan), DATA["滑块位移"].replace([np.inf, -np.inf], np.nan), s = 5.0, alpha =1.0,edgecolor="black",linewidth= 0.0)
    plt.show() 
    fig, ax = plt.subplots()
    ax.set_xlabel( "曲柄转角")
    ax.set_ylabel( "曲柄扭矩" )
    ax.set_xlim(min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    ax.set_ylim( min(DATA['曲柄扭矩'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['曲柄扭矩'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['曲柄扭矩'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['曲柄扭矩'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    #- PLOT ------------------ 
    ax.scatter(DATA["曲柄转角"].replace([np.inf, -np.inf], np.nan), DATA["曲柄扭矩"].replace([np.inf, -np.inf], np.nan), s = 5.0, alpha =1.0,edgecolor="black",linewidth= 0.0)
    plt.show() 
    fig, ax = plt.subplots()
    ax.set_xlabel( "时间")
    ax.set_ylabel( "滑块速度" )
    ax.set_xlim(min(DATA['时间'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['时间'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['时间'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['时间'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    ax.set_ylim( min(DATA['滑块速度'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['滑块速度'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['滑块速度'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['滑块速度'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    #- PLOT ------------------ 
    ax.scatter(DATA["时间"].replace([np.inf, -np.inf], np.nan), DATA["滑块速度"].replace([np.inf, -np.inf], np.nan), s = 5.0, alpha =1.0,edgecolor="black",linewidth= 0.0)
    plt.show() 
    fig, ax = plt.subplots()
    ax.set_xlabel( "曲柄转角")
    ax.set_ylabel( "连杆支撑力bx" )
    ax.set_xlim(min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['曲柄转角'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    ax.set_ylim( min(DATA['连杆支撑力bx'].replace([np.inf, -np.inf], np.nan ).dropna() ) - abs( min(DATA['连杆支撑力bx'].replace([np.inf, -np.inf], np.nan ).dropna() )/10), max(DATA['连杆支撑力bx'].replace([np.inf, -np.inf], np.nan).dropna()) + abs(max(DATA['连杆支撑力bx'].replace([np.inf, -np.inf], np.nan).dropna())/10)  )
    #- PLOT ------------------ 
    ax.scatter(DATA["曲柄转角"].replace([np.inf, -np.inf], np.nan), DATA["连杆支撑力bx"].replace([np.inf, -np.inf], np.nan), s = 5.0, alpha =1.0,edgecolor="black",linewidth= 0.0)
    plt.show() 